// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (Robotics and Perception Group, University of Zurich, Switzerland).
//
// SVO is free software: you can redistribute it and/or modify it under the
// terms of the GNU General Public License as published by the Free Software
// Foundation, either version 3 of the License, or any later version.
//
// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
// details.
//
// You should have received a copy of the GNU General Public License
// along with this program.  If not, see <http://www.gnu.org/licenses/>.

#ifndef SVO_MATCHER_H_
#define SVO_MATCHER_H_

#include <svo/global.h>

namespace vk {
class AbstractCamera;
namespace patch_score {
template <int HALF_PATCH_SIZE>
class ZMSSD;
}
}  // namespace vk

namespace svo {

class Point;
class Frame;
class Feature;

/// Warp a patch from the reference view to the current view.
namespace warp {

void getWarpMatrixAffine(const vk::AbstractCamera& cam_ref,
                         const vk::AbstractCamera& cam_cur,
                         const Vector2d& px_ref,
                         const Vector3d& f_ref,
                         const double depth_ref,
                         const Sophus::SE3d& T_cur_ref,
                         const int level_ref,
                         Matrix2d& A_cur_ref);

int getBestSearchLevel(const Matrix2d& A_cur_ref, const int max_level);

void warpAffine(const Matrix2d& A_cur_ref,
                const cv::Mat& img_ref,
                const Vector2d& px_ref,
                const int level_ref,
                const int level_cur,
                const int halfpatch_size,
                uint8_t* patch);

}  // namespace warp

/// Patch-matcher for reprojection-matching and epipolar search in
/// triangulation.
class Matcher {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  static const int halfpatch_size_ = 4;
  static const int patch_size_ = 8;

  typedef vk::patch_score::ZMSSD<halfpatch_size_> PatchScore;

  struct Options {
    bool align_1d;  //!< in epipolar search: align patch 1D along epipolar line
    int align_max_iter;  //!< number of iterations for aligning the feature
                         //!< patches in gauss newton
    double
        max_epi_length_optim;  //!< max length of epipolar line to skip epipolar
                               //!< search and directly go to img align
    size_t max_epi_search_steps;  //!< max number of evaluations along epipolar
                                  //!< line
    bool subpix_refinement;  //!< do gauss newton feature patch alignment after
                             //!< epipolar search
    bool epi_search_edgelet_filtering;
    double epi_search_edgelet_max_angle;
    Options()
        : align_1d(false),
          align_max_iter(10),
          max_epi_length_optim(2.0),
          max_epi_search_steps(1000),
          subpix_refinement(true),
          epi_search_edgelet_filtering(true),
          epi_search_edgelet_max_angle(0.7) {}
  } options_;

  uint8_t patch_[patch_size_ * patch_size_] __attribute__((aligned(16)));
  uint8_t patch_with_border_[(patch_size_ + 2) * (patch_size_ + 2)]
      __attribute__((aligned(16)));
  Matrix2d A_cur_ref_;  //!< affine warp matrix
  Vector2d epi_dir_;
  double epi_length_;  //!< length of epipolar line segment in pixels (only used
                       //!< for epipolar search)
  double h_inv_;       //!< hessian of 1d image alignment along epipolar line
  int search_level_;
  bool reject_;
  Feature* ref_ftr_;
  Vector2d px_cur_;

  Matcher() = default;
  ~Matcher() = default;

  /// Find a match by directly applying subpix refinement.
  /// IMPORTANT! This function assumes that px_cur is already set to an estimate
  /// that is within ~2-3 pixel of the final result!
  bool findMatchDirect(const Point& pt, const Frame& frame, Vector2d& px_cur);

  /// Find a match by searching along the epipolar line without using any
  /// features.
  bool findEpipolarMatchDirect(const Frame& ref_frame,
                               const Frame& cur_frame,
                               const Feature& ref_ftr,
                               const double d_estimate,
                               const double d_min,
                               const double d_max,
                               double& depth);

  void createPatchFromPatchWithBorder();
};

}  // namespace svo

#endif  // SVO_MATCHER_H_
